///////////////////////////////
// servos mechanism
// 0.21
// work in microSecond for better precision ! 500 for 90 :)
// 500 = 0°, 1500=90°, 2500=180°

int deg2ms(int deg)
{
  //return map(deg, 0, 180, 500, 2500);
  return 500+((deg/180.0)*2000);
}

int init_servos()
{
  for(int i=0; i<N_SERVOS; i++)
  {
    if(servo_pin[i] != -1)
    {
      servos[i].attach(servo_pin[i]);
    }
  }
}

void update_servos()
{
  for(int i=0; i<N_SERVOS; i++)
  {
    if(servo_update[i])
    {
      int ms = servo_pos[i]+servo_cal[1][i];
      
      //if(servo_pos[i]<1500) ms = map(servo_pos[i], 500, 1500, 500+servo_cal[0][i], 1500+servo_cal[1][i]);
      //else if(servo_pos[i]>1500) ms = map(servo_pos[i], 1500, 2500, 1500+servo_cal[1][i], 2500+servo_cal[2][i]);
      
      servos[i].writeMicroseconds(ms);
      servo_update[i] = false;
    }
  }
}

int set_servo_position(int iInd, int iPos)
{
  for(int i=0; i<N_SERVOS; i++)
  {
    if(servo_ind[i] == iInd)
    {
      int p = iPos - servo_pos[i];
      servo_pos[i] = iPos;
      servo_update[i] = true;
      return (p<0?-p:p);
    }
  }
  Serial.print("cannot find servo '");
  Serial.print(iInd);
  Serial.println("' !");
  return -1;
}

